#include "epuckDriver/epuckDriver.hpp"
#include <string>
#include <cstddef>
#include "epuckDriver/configFile.hpp"
#include <signal.h>
#include "ros/callback_queue.h"

int main(int argc,char **argv){

    ConfigFile cfgFile("epuck.xml");
    cfgFile.readConfigEpucks();    
   
    InfoEpuck info;
    int numberEpuck;

    if(argc < 2){
        ROS_ERROR("Enter with the id of e-puck");
        return -1;
    }
    else{
        numberEpuck = atoi(argv[1]) - 1 ; 
        
        if( numberEpuck < 0 ){
            ROS_ERROR("Enter with number > 0 ");
            return -1;
        } 
        else
          info = cfgFile.getInfoNodeID(numberEpuck);
          info.id = argv[1];            
    }

    ros::init(argc,argv,info.node.c_str());
    printf("%s",info.node.c_str());
    EpuckDriver epuck(info.id,info.port);

    ros::CallbackQueue queueVelocity;
//    ros::CallbackQueue queueLEDFront;
//    ros::CallbackQueue queueLEDBody;
//    ros::CallbackQueue queueLEDRing;
    ros::CallbackQueue queueServiceServer;

    ros::NodeHandle nServiceServer;
    ros::NodeHandle nVelocity;
//    ros::NodeHandle nLEDFront;
//    ros::NodeHandle nLEDBody;
//    ros::NodeHandle nLEDRing;

    nServiceServer.setCallbackQueue(&queueServiceServer);
    nVelocity.setCallbackQueue(&queueVelocity);
//    nLEDFront.setCallbackQueue(&queueLEDFront);
//    nLEDBody.setCallbackQueue(&queueLEDBody);
//    nLEDRing.setCallbackQueue(&queueLEDRing);
 
    ros::Subscriber subVelocity;   
    ros::Subscriber subCommand;   
  //  ros::Subscriber subFrontLED;   
  //  ros::Subscriber subBodyLED;   
  //  ros::Subscriber subRingLED;   
    
    ros::ServiceServer setOdometry;
    ros::ServiceServer resetOdometry;
    ros::ServiceServer getGeometry;
    ros::ServiceServer getIRPose;
    ros::ServiceServer getOdometry;

    if(info.cmdVelocity){
        subVelocity = 
nVelocity.subscribe<geometry_msgs::Twist>(epuck.WhereSubVelocity(),1,&EpuckDriver::VelocityCallBack,&epuck);
//        subCommand =
//nVelocity.subscribe<epuckDriver::CommandSubscriberMsg>(epuck.WhereSubCommand(),1,&EpuckDriver::CommandCallBack,&epuck);
        setOdometry =
nServiceServer.advertiseService(epuck.WhereServerSetOdom(),&EpuckDriver::SetOdometry,&epuck);
        resetOdometry =      
nServiceServer.advertiseService(epuck.WhereServerResetOdom(),&EpuckDriver::ResetOdometry,&epuck);
        getGeometry =   
nServiceServer.advertiseService(epuck.WhereServerGetGeom(),&EpuckDriver::GetGeometry,&epuck);
        getOdometry =    
nServiceServer.advertiseService(epuck.WhereServerGetOdom(),&EpuckDriver::GetOdometry,&epuck);

    }

//    if(info.cmdLEDs){
//        subFrontLED =
//nLEDFront.subscribe<std_msgs::Bool>(epuck.WhereSubFrontLED(),1,&EpuckDriver::FrontLEDCallBack,&epuck);
//        subBodyLED =
//nLEDBody.subscribe<std_msgs::Bool>(epuck.WhereSubBodyLED(),1,&EpuckDriver::BodyLEDCallBack,&epuck);
//        subRingLED =
//nLEDRing.subscribe<epuckDriver::BoolMsg>(epuck.WhereSubRingLED(),1,&EpuckDriver::RingLEDCallBack,&epuck);
//    }

    if(info.cmdIR)
        getIRPose =   
nServiceServer.advertiseService(epuck.WhereServerGetIRPose(),&EpuckDriver::GetIRPose2D,&epuck);
        
 //   queueVelocity.callOne(ros::WallDuration(0)); 
 //   queueServiceServer.callOne(ros::WallDuration(0)); 
 //   queueLEDFront.callOne(ros::WallDuration(0)); 
 //   queueLEDBody.callOne(ros::WallDuration(0)); 
//    queueLEDRing.callOne(ros::WallDuration(0)); 
   
//    queueServiceServer.callAvailable(ros::WallDuration(0)); 
//    queueLEDFront.callAvailable(ros::WallDuration(0));
//    queueVelocity.callAvailable(ros::WallDuration(0));
    
    ros::AsyncSpinner spinnerVelocity(1,&queueVelocity);
    ros::AsyncSpinner spinnerServiceServer(1,&queueServiceServer);
//    ros::AsyncSpinner spinnerFront(1,&queueLEDFront);
//    ros::AsyncSpinner spinnerBody(1,&queueLEDBody);
//    ros::AsyncSpinner spinnerRing(1,&queueLEDRing);

    spinnerVelocity.start();
    spinnerServiceServer.start();
//    spinnerFront.start();
//    spinnerBody.start();
//    spinnerRing.start();
        
    if(epuck.Start() == -1 );
//        return -1;
    
    ros::Rate loopRate(10);

    
    ROS_INFO("%s ok",info.node.c_str());

    while(ros::ok){  
        epuck.Publish(info.cmdVelocity,info.cmdIR,info.cmdCamera); 
        loopRate.sleep();
    }
    
    ros::waitForShutdown();
    
    return 0;
}
